<!-- HTML header for doxygen 1.8.6-->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<title>OpenCV: Pose Estimation</title>
<link href="../../opencv.ico" rel="shortcut icon" type="image/x-icon" />
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<script type="text/javascript" src="../../tutorial-utils.js"></script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/x-mathjax-config">
  MathJax.Hub.Config({
    extensions: ["tex2jax.js", "TeX/AMSmath.js", "TeX/AMSsymbols.js"],
    jax: ["input/TeX","output/HTML-CSS"],
});
//<![CDATA[
MathJax.Hub.Config(
{
  TeX: {
      Macros: {
          matTT: [ "\\[ \\left|\\begin{array}{ccc} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{array}\\right| \\]", 9],
          fork: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ \\end{array} \\right.", 4],
          forkthree: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ \\end{array} \\right.", 6],
          forkfour: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ #7 & \\mbox{#8}\\\\ \\end{array} \\right.", 8],
          vecthree: ["\\begin{bmatrix} #1\\\\ #2\\\\ #3 \\end{bmatrix}", 3],
          vecthreethree: ["\\begin{bmatrix} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{bmatrix}", 9],
          cameramatrix: ["#1 = \\begin{bmatrix} f_x & 0 & c_x\\\\ 0 & f_y & c_y\\\\ 0 & 0 & 1 \\end{bmatrix}", 1],
          distcoeffs: ["(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \\tau_x, \\tau_y]]]]) \\text{ of 4, 5, 8, 12 or 14 elements}"],
          distcoeffsfisheye: ["(k_1, k_2, k_3, k_4)"],
          hdotsfor: ["\\dots", 1],
          mathbbm: ["\\mathbb{#1}", 1],
          bordermatrix: ["\\matrix{#1}", 1]
      }
  }
}
);
//]]>
</script><script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js"></script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
<link href="../../stylesheet.css" rel="stylesheet" type="text/css"/>
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<!--#include virtual="/google-search.html"-->
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectlogo"><img alt="Logo" src="../../opencv-logo-small.png"/></td>
  <td style="padding-left: 0.5em;">
   <div id="projectname">OpenCV
   &#160;<span id="projectnumber">4.5.2</span>
   </div>
   <div id="projectbrief">Open Source Computer Vision</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
<script type="text/javascript" src="../../menudata.js"></script>
<script type="text/javascript" src="../../menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('../../',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="../../d6/d00/tutorial_py_root.html">OpenCV-Python Tutorials</a></li><li class="navelem"><a class="el" href="../../d9/db7/tutorial_py_table_of_contents_calib3d.html">Camera Calibration and 3D Reconstruction</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">Pose Estimation </div>  </div>
</div><!--header-->
<div class="contents">
<div class="textblock"><h2>Goal </h2>
<p>In this section,</p><ul>
<li>We will learn to exploit calib3d module to create some 3D effects in images.</li>
</ul>
<h2>Basics </h2>
<p>This is going to be a small section. During the last session on camera calibration, you have found the camera matrix, distortion coefficients etc. Given a pattern image, we can utilize the above information to calculate its pose, or how the object is situated in space, like how it is rotated, how it is displaced etc. For a planar object, we can assume Z=0, such that, the problem now becomes how camera is placed in space to see our pattern image. So, if we know how the object lies in the space, we can draw some 2D diagrams in it to simulate the 3D effect. Let's see how to do it.</p>
<p>Our problem is, we want to draw our 3D coordinate axis (X, Y, Z axes) on our chessboard's first corner. X axis in blue color, Y axis in green color and Z axis in red color. So in-effect, Z axis should feel like it is perpendicular to our chessboard plane.</p>
<p>First, let's load the camera matrix and distortion coefficients from the previous calibration result. </p><div class="fragment"><div class="line"><span class="keyword">import</span> numpy <span class="keyword">as</span> np</div><div class="line"><span class="keyword">import</span> cv2 <span class="keyword">as</span> cv</div><div class="line"><span class="keyword">import</span> glob</div><div class="line"></div><div class="line"><span class="comment"># Load previously saved data</span></div><div class="line">with np.load(<span class="stringliteral">&#39;B.npz&#39;</span>) <span class="keyword">as</span> X:</div><div class="line">    mtx, dist, _, _ = [X[i] <span class="keywordflow">for</span> i <span class="keywordflow">in</span> (<span class="stringliteral">&#39;mtx&#39;</span>,<span class="stringliteral">&#39;dist&#39;</span>,<span class="stringliteral">&#39;rvecs&#39;</span>,<span class="stringliteral">&#39;tvecs&#39;</span>)]</div></div><!-- fragment --><p> Now let's create a function, draw which takes the corners in the chessboard (obtained using <b><a class="el" href="../../d9/d0c/group__calib3d.html#ga93efa9b0aa890de240ca32b11253dd4a" title="Finds the positions of internal corners of the chessboard. ">cv.findChessboardCorners()</a></b>) and <b>axis points</b> to draw a 3D axis. </p><div class="fragment"><div class="line"><span class="keyword">def </span>draw(img, corners, imgpts):</div><div class="line">    corner = tuple(corners[0].ravel())</div><div class="line">    img = <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga7078a9fae8c7e7d13d24dac2520ae4a2">cv.line</a>(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)</div><div class="line">    img = <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga7078a9fae8c7e7d13d24dac2520ae4a2">cv.line</a>(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)</div><div class="line">    img = <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga7078a9fae8c7e7d13d24dac2520ae4a2">cv.line</a>(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)</div><div class="line">    <span class="keywordflow">return</span> img</div></div><!-- fragment --><p> Then as in previous case, we create termination criteria, object points (3D points of corners in chessboard) and axis points. Axis points are points in 3D space for drawing the axis. We draw axis of length 3 (units will be in terms of chess square size since we calibrated based on that size). So our X axis is drawn from (0,0,0) to (3,0,0), so for Y axis. For Z axis, it is drawn from (0,0,0) to (0,0,-3). Negative denotes it is drawn towards the camera. </p><div class="fragment"><div class="line">criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)</div><div class="line">objp = np.zeros((6*7,3), np.float32)</div><div class="line">objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)</div><div class="line"></div><div class="line">axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)</div></div><!-- fragment --><p> Now, as usual, we load each image. Search for 7x6 grid. If found, we refine it with subcorner pixels. Then to calculate the rotation and translation, we use the function, <b><a class="el" href="../../d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e" title="Finds an object pose from 3D-2D point correspondences using the RANSAC scheme. ">cv.solvePnPRansac()</a></b>. Once we those transformation matrices, we use them to project our <b>axis points</b> to the image plane. In simple words, we find the points on image plane corresponding to each of (3,0,0),(0,3,0),(0,0,3) in 3D space. Once we get them, we draw lines from the first corner to each of these points using our draw() function. Done !!! </p><div class="fragment"><div class="line"><span class="keywordflow">for</span> fname <span class="keywordflow">in</span> glob.glob(<span class="stringliteral">&#39;left*.jpg&#39;</span>):</div><div class="line">    img = <a class="code" href="../../d4/da8/group__imgcodecs.html#ga288b8b3da0892bd651fce07b3bbd3a56">cv.imread</a>(fname)</div><div class="line">    gray = <a class="code" href="../../d8/d01/group__imgproc__color__conversions.html#ga397ae87e1288a81d2363b61574eb8cab">cv.cvtColor</a>(img,cv.COLOR_BGR2GRAY)</div><div class="line">    ret, corners = <a class="code" href="../../d9/d0c/group__calib3d.html#ga93efa9b0aa890de240ca32b11253dd4a">cv.findChessboardCorners</a>(gray, (7,6),<span class="keywordtype">None</span>)</div><div class="line"></div><div class="line">    <span class="keywordflow">if</span> ret == <span class="keyword">True</span>:</div><div class="line">        corners2 = <a class="code" href="../../dd/d1a/group__imgproc__feature.html#ga354e0d7c86d0d9da75de9b9701a9a87e">cv.cornerSubPix</a>(gray,corners,(11,11),(-1,-1),criteria)</div><div class="line"></div><div class="line">        <span class="comment"># Find the rotation and translation vectors.</span></div><div class="line">        ret,rvecs, tvecs = <a class="code" href="../../d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d">cv.solvePnP</a>(objp, corners2, mtx, dist)</div><div class="line"></div><div class="line">        <span class="comment"># project 3D points to image plane</span></div><div class="line">        imgpts, jac = <a class="code" href="../../d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c">cv.projectPoints</a>(axis, rvecs, tvecs, mtx, dist)</div><div class="line"></div><div class="line">        img = draw(img,corners2,imgpts)</div><div class="line">        <a class="code" href="../../df/d24/group__highgui__opengl.html#gaae7e90aa3415c68dba22a5ff2cefc25d">cv.imshow</a>(<span class="stringliteral">&#39;img&#39;</span>,img)</div><div class="line">        k = <a class="code" href="../../d7/dfc/group__highgui.html#ga5628525ad33f52eab17feebcfba38bd7">cv.waitKey</a>(0) &amp; 0xFF</div><div class="line">        <span class="keywordflow">if</span> k == ord(<span class="stringliteral">&#39;s&#39;</span>):</div><div class="line">            <a class="code" href="../../d4/da8/group__imgcodecs.html#gabbc7ef1aa2edfaa87772f1202d67e0ce">cv.imwrite</a>(fname[:6]+<span class="stringliteral">&#39;.png&#39;</span>, img)</div><div class="line"></div><div class="line"><a class="code" href="../../d7/dfc/group__highgui.html#ga6b7fc1c1a8960438156912027b38f481">cv.destroyAllWindows</a>()</div></div><!-- fragment --><p> See some results below. Notice that each axis is 3 squares long.:</p>
<div class="image">
<img src="../../pose_1.jpg" alt="pose_1.jpg"/>
<div class="caption">
image</div></div>
 <h3>Render a Cube</h3>
<p>If you want to draw a cube, modify the draw() function and axis points as follows.</p>
<p>Modified draw() function: </p><div class="fragment"><div class="line"><span class="keyword">def </span>draw(img, corners, imgpts):</div><div class="line">    imgpts = np.int32(imgpts).reshape(-1,2)</div><div class="line"></div><div class="line">    <span class="comment"># draw ground floor in green</span></div><div class="line">    img = <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga746c0625f1781f1ffc9056259103edbc">cv.drawContours</a>(img, [imgpts[:4]],-1,(0,255,0),-3)</div><div class="line"></div><div class="line">    <span class="comment"># draw pillars in blue color</span></div><div class="line">    <span class="keywordflow">for</span> i,j <span class="keywordflow">in</span> zip(range(4),range(4,8)):</div><div class="line">        img = <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga7078a9fae8c7e7d13d24dac2520ae4a2">cv.line</a>(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3)</div><div class="line"></div><div class="line">    <span class="comment"># draw top layer in red color</span></div><div class="line">    img = <a class="code" href="../../d6/d6e/group__imgproc__draw.html#ga746c0625f1781f1ffc9056259103edbc">cv.drawContours</a>(img, [imgpts[4:]],-1,(0,0,255),3)</div><div class="line"></div><div class="line">    <span class="keywordflow">return</span> img</div></div><!-- fragment --><p> Modified axis points. They are the 8 corners of a cube in 3D space: </p><div class="fragment"><div class="line">axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],</div><div class="line">                   [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])</div></div><!-- fragment --><p> And look at the result below:</p>
<div class="image">
<img src="../../pose_2.jpg" alt="pose_2.jpg"/>
<div class="caption">
image</div></div>
<p> If you are interested in graphics, augmented reality etc, you can use OpenGL to render more complicated figures.</p>
<h2>Additional Resources </h2>
<h2>Exercises </h2>
</div></div><!-- contents -->
<!-- HTML footer for doxygen 1.8.6-->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated on Fri Apr 2 2021 11:36:37 for OpenCV by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="../../doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
<script type="text/javascript">
//<![CDATA[
addTutorialsButtons();
//]]>
</script>
</body>
</html>
